package com.google.android.apps.skipbot;

/**
 * Class that represents the leg of a skipper robot.
 */
public class SkipperLeg {
  private final int legIndex;

  private final Servo hipServo;
  private final Servo shoulderServo;
  private final Servo elbowServo;

  private final SkipperIKCalculator calculator;

  public SkipperLeg(int legIndex, ServoProvider servoProvider,
      SkipperRobotConfigInterface robotConfig, SkipperIKCalculator calculator) {
    this.legIndex = legIndex;
    this.hipServo = servoProvider.getServo(robotConfig.getHipServoIndex(legIndex));
    this.shoulderServo = servoProvider.getServo(robotConfig.getShoulderServoIndex(legIndex));
    this.elbowServo = servoProvider.getServo(robotConfig.getElbowServoIndex(legIndex));
    this.calculator = calculator;
  }

  /**
   * Creates an array of SkipperLegs based of the provided robot config and
   * referencing the servos provided by the servo provider.
   * 
   * @param servoProvider gives acces to servos to be referenced by the legs
   * @param robotConfig gives the configuration of the robot
   * @param calculator an inverse kinematics calculator for the robot
   * @return an array of SkipperLegs ordered and configured based on the robot
   *         config
   */
  public static SkipperLeg[] createLegArray(ServoProvider servoProvider,
      SkipperRobotConfigInterface robotConfig, SkipperIKCalculator calculator) {
    int numLegs = robotConfig.getNumLegs();
    SkipperLeg[] legs = new SkipperLeg[numLegs];
    for (int i = 0; i < numLegs; ++i) {
      legs[i] = new SkipperLeg(i, servoProvider, robotConfig, calculator);
    }
    return legs;
  }

  /**
   * Moves the specified leg to the XYZ position given in inches.
   * 
   * @param position length 3 array of x, y, z, coordinates in inches relative
   *        to bottom center of the robot
   */
  public void moveTo(double[] position) {
    int[] angles = getAngles(position);
    hipServo.moveServo(angles[0]);
    shoulderServo.moveServo(angles[1]);
    elbowServo.moveServo(angles[2]);
  }

  /**
   * Gets the angles for the leg to the XYZ position given in inches.
   * 
   * @param position length 3 array of x, y, z, coordinates in inches relative
   *        to bottom center of the robot
   */
  public int[] getAngles(double[] position) {
    return calculator.getLegAngles(legIndex, position);
  }
}
